#include "filter.h"

void LPFSetCutoffFreq(PF_STRUCT *lpf,float sample_freq, float cutoff_freq, FP32 data)
{
	FP32 tao;
	FP32 sample_t;
	tao = 1.0f/(2.0f*M_PI_F*cutoff_freq);
	sample_t = 1.0f/sample_freq;
	lpf->factor = sample_t/(sample_t + tao);
	lpf->last_out_1 = data;
}

float LPFApply(PF_STRUCT *lpf,float sample)
{
	FP32 out;
	out = lpf->factor*sample + (1.0f-lpf->factor)*lpf->last_out_1;
	lpf->last_out_1 = out;
	return out;
}

void HPFSetCutoffFreq(PF_STRUCT *hpf,float sample_freq, float cutoff_freq, FP32 data)
{
	FP32 tao;
	FP32 sample_t;
	tao = 1.0f/(2.0f*M_PI_F*cutoff_freq);
	sample_t = 1.0f/sample_freq;
	hpf->factor = tao/(sample_t + tao);
	hpf->last_out_1 = data;
	hpf->last_sample_1=data;
}

float HPFApply(PF_STRUCT *hpf,float sample)
{
	FP32 out;
	out = hpf->factor*(sample - hpf->last_sample_1 + hpf->last_out_1);
	hpf->last_out_1 = out;
	hpf->last_sample_1=sample;
	return out;
}
